#include <buola/scene/cbone.h>
#include <buola/scene/crendercontext.h>
#include <buola/scene/cskeleton.h>

#include <buola/gl.h>

namespace buola { namespace scene {

CBone::CBone(const std::string &pName,const mat::CVec3d &pOrigin,const mat::CVec3d &pEndPoint,const mat::C3DRotation &pOrientation)
    :   mParent(nullptr)
    ,   mName(pName)
    ,   mOrigin(pOrigin)
    ,   mEndPoint(pEndPoint)
    ,   mOrientation(pOrientation)
{
    mTransform=new CBoneTransform(this);
}

CBone::~CBone()
{
    
}

void CBone::AddChild(PBone pBone)
{
    mChildren.push_back(pBone);
    pBone->mParent=this;
    mTransform->AddChild(pBone->mTransform);
}

void CBone::AddJoint(EJointType pType,const mat::CVec3d &pAxis,const mat::CVec3d &pZero)
{
    SJoint *lJoint=new SJoint;
    lJoint->mType=pType;
    lJoint->mValue=0.0;
    lJoint->mAxis=pAxis;
    lJoint->mZero=pZero;
    lJoint->mAngles=false;
    lJoint->mSpheres=false;

    mJoints.push_back(lJoint);
    
    mTransform->InvalidateTransform();
}

void CBone::AddJoint(EJointType pType,const mat::CVec3d &pAxis,const mat::CVec3d &pZero,double pA,double pB,double pC,double pD)
{
    SJoint *lJoint=new SJoint;
    lJoint->mType=pType;
    lJoint->mValue=0.0;
    lJoint->mAxis=pAxis;
    lJoint->mZero=pZero;
    lJoint->mAngles=true;
    lJoint->mA=pA;
    lJoint->mB=pB;
    lJoint->mC=pC;
    lJoint->mD=pD;
    lJoint->mSpheres=false;

    mJoints.push_back(lJoint);

    mTransform->InvalidateTransform();
}

void CBone::AddJoint(EJointType pType,const mat::CVec3d &pAxis,const mat::CVec3d &pZero,double pA,double pB,double pC,double pD,
                     const mat::CMat34d &pInner,const mat::CMat34d &pOuter)
{
    SJoint *lJoint=new SJoint;
    lJoint->mType=pType;
    lJoint->mValue=0.0;
    lJoint->mAxis=pAxis;
    lJoint->mZero=pZero;
    lJoint->mAngles=true;
    lJoint->mA=pA;
    lJoint->mB=pB;
    lJoint->mC=pC;
    lJoint->mD=pD;

    mat::CMat34d lTransform=mat::translation_3d(mOrigin)*mat::rotation_3d(mOrientation)*mat::translation_3d(-mOrigin);
    
    lJoint->mSpheres=true;
    lJoint->mInner=inverse(lTransform*pInner);
    lJoint->mOuter=inverse(lTransform*pOuter);

    mJoints.push_back(lJoint);

    mTransform->InvalidateTransform();
}

void CBone::SetJointValue(EJointType pType,double pValue)
{
    SJoint *lJoint=nullptr;
    
    for(int i=0;i<mJoints.size();i++)
    {
        if(mJoints[i]->mType==pType)
        {
            lJoint=mJoints[i];
            break;
        }
    }

    if(!lJoint)
    {
        msg_warn() << "joint type not found in SetJointValue\n";
        return;
    }

    lJoint->mValue=pValue;
    mTransform->InvalidateTransform();
}

double CBone::GetJointValue(EJointType pType)
{
    SJoint *lJoint=nullptr;
    
    for(int i=0;i<mJoints.size();i++)
    {
        if(mJoints[i]->mType==pType)
        {
            lJoint=mJoints[i];
            break;
        }
    }

    if(!lJoint)
    {
        msg_warn() << "joint type not found in SetJointValue\n";
        return 0.0;
    }

    return lJoint->mValue;
}

double CBone::CalcBlendWeight(const mat::CVec3d &pPos)
{
    SJoint *lBend=nullptr;
    for(int i=0;i<mJoints.size();i++)
    {
        if(mJoints[i]->mType==JOINTTYPE_BEND)
        {
            lBend=mJoints[i];
        }
    }

    if(!lBend) return 0.0;

    mat::CVec3d lDirection=pPos-mOrigin;
    //we project it into the plane perpendicular to the rotation which contains the origin
    mat::CVec3d lAxis=mOrientation*lBend->mAxis;
    mat::CVec3d lZero=mOrientation*lBend->mZero;
    lDirection=lDirection-lAxis*dot(lDirection,lAxis);

    double lSphereBlend=1.0;
    if(lBend->mSpheres)
    {
        double lIn=norm(lBend->mInner*pPos);
        double lOut=norm(lBend->mOuter*pPos);

        if(lOut>1.0) //outside outer sphere... don't use
        {
            return 0.0;
        }

        if(lIn>1.0) //outside inner sphere... blend
        {
            double lOne=lOut/lIn;
            lSphereBlend=1.0-(lOut-lOne)/(1.0-lOne);
        }
    }

    double lAngle=angle(lZero,lDirection,lAxis);

    double lAngleBlend=1.0;

    if(lBend->mAngles)
    {
        if(is_angle_between(lAngle,lBend->mC,lBend->mB)) //inclusion zone
        {
            lAngleBlend=1.0;
        }
        else if(is_angle_between(lAngle,lBend->mA,lBend->mD)) //exclusion zone
        {
            return 0.0;
        }
        else if(is_angle_between(lAngle,lBend->mB,lBend->mA)) //blend zone 1 (B<A)
        {
            double lPos=angle_diff(lBend->mB,lAngle)/
                        angle_diff(lBend->mB,lBend->mA);
            lAngleBlend=1.0-lPos;
        }
        else //blend zone 2 (D<C when normalized)
        {
            assert(is_angle_between(lAngle,lBend->mD,lBend->mC+2*M_PI));
            double lPos=angle_diff(lBend->mD,lAngle)/
                        angle_diff(lBend->mD,lBend->mC+2*M_PI);
            lAngleBlend=lPos;
        }
    }

    return lAngleBlend*lSphereBlend;
}

void CBone::AssignIndex(CSkeleton::SAssignIndexContext &pContext)
{
    mBoneIndex=pContext.mNextIndex++;

///\todo
//    pContext.mMesh->AssignSkinIndex(mName,mBoneIndex);
    
    for(int i=0;i<mChildren.size();i++)
        mChildren[i]->AssignIndex(pContext);
}

void CBone::SkinMesh(CSkeleton::SSkinMeshContext &pContext)
{
///\todo
//    pContext.mMesh->AssignSkinIndices(mName,this);

    for(int i=0;i<mChildren.size();i++)
        mChildren[i]->SkinMesh(pContext);
}

void CBone::FillMatrices(CSkeleton::SFillMatricesContext &pContext)
{
    mat::CMat34d lOldModelMatrix=pContext.mModelMatrix;
    pContext.mModelMatrix=mTransform->GetWorldTransform();
    pContext.mMatrices.push_back(pContext.mViewMatrix*pContext.mModelMatrix);
    for(int i=0;i<mChildren.size();i++)
        mChildren[i]->FillMatrices(pContext);
    pContext.mModelMatrix=lOldModelMatrix;
}

void CBone::Render(CRenderContext &pContext,CSkeleton *pSkeleton)
{
    mat::CMat34d lOldModelMatrix=pContext.mModelMatrix;
    pContext.mModelMatrix=mTransform->GetWorldTransform();
    pSkeleton->GetMesh()->Render(mName);
    for(int i=0;i<mChildren.size();i++)
        mChildren[i]->Render(pContext,pSkeleton);
    pContext.mModelMatrix=lOldModelMatrix;
}

mat::CVec3d CBone::GetTransformedOrigin()
{
    return mTransform->GetWorldTransform()*mOrigin;
}

mat::CVec3d CBone::GetTransformedEndPoint()
{
    return mTransform->GetWorldTransform()*mEndPoint;
}

/*namespace gl*/ } /*namespace buola*/ }
